#include <math.h>
#include <values.h>
#include <assert.h>
#include <stdlib.h>
#include "fobjects.h"
#include "remez.h"

/******************************************************************************
 * Delta - current approximation error.                                       *
 * Q - convergence parameter, starts with value of 1 and should fall to 0 as  *
 *     optimization goes                                                      *
 * RequiredQ - ending value for Q                                             *
 * S - grid density (how many points are searched between any two initial     *
 *     extremals)                                                             *
 * FIR - holds information about filter we want to design (bands edges,       *
 *       transmittance and relative error ratio)                              *
 * Extrm - set of current extremals                                           *
 * cosExtrm - cosine functions evaluated on current extremals                 *
 * Alfa - ALFA coefficients                                                   *
 * Beta - BETA coefficients                                                   *
 * C - C coefficients                                                         *
 * NewExtrm - set of potential new extremals                                  *
 * r - number of coefficients we are looking for                              *
 * W - space between initial extremals in each band                           *
 * R - if distance between two adjacent extremals exceeds R * W[i], we do     *
 *     exhaustive search between them                                         *
 * Skipper - ratio of band length to skip from 0 or pi if neccessary          *
 * FType - filter type (odd, even, symmetrical or not, differentiator)        *
 * FLength - filter length                                                    *
 * __Q - pointer to Q(w) function                                             *
 * __dD - pointer to the first derivative of desired amplitude response       *
 * __d2D - pointer to the second derivative of desired amplitude response     *
 ******************************************************************************/

Float Delta, Q, RequiredQ = 0.01;
unsigned int S = 16;

static Filter FIR;
static Array Extrm, cosExtrm, Alfa, Beta, C, NewExtrm;
static int r;
static Vector W;

const Float R = 1.5;
const Float Skipper = 0.1;

static FilterTypes FType;
static int FLength;

static Float (*__Q)(Float);
static Float (*__dD)(Float);
static Float (*__d2D)(Float);

/******************************************************************************
 * Desired amplitude response, modified as neccessery for optimization        *
 ******************************************************************************/
static Float Desire(Float w)
{
  for(int i = 1; i <= GetBandsNo(FIR); i++)
    if(w > FIR[i].GetLow() - 1e-6 && w < FIR[i].GetHigh() + 1e-6)
      return (FIR[i].GetTrans() / (*__Q)(w));
  fail(Desire domain error);
  return (0); // never reached, but we don't like warnings
}

/******************************************************************************
 * Weight function, modified as neccessery for optimization                   *
 ******************************************************************************/
static Float Weight(Float w)
{
  for(int i = 1; i <= GetBandsNo(FIR); i++)
    if(w > FIR[i].GetLow() - 1e-6 && w < FIR[i].GetHigh() + 1e-6)
      return (FIR[i].GetErrorRatio() * (*__Q)(w));
  fail(Weight domain error);
  return (0); // never reached, but we don't like warnings
}

/******************************************************************************
 * Q(w) functions for four cases of filter design                             *
 ******************************************************************************/
static Float Q1(Float)
{
  return (1);
}

static Float Q2(Float x)
{
  return (sin(x));
}

static Float Q3(Float x)
{
  return (cos(x / 2));
}

static Float Q4(Float x)
{
  return (sin(x / 2));
}

static Float Q5(Float x)
{
  return (Eq(x, 0) ? 1 : sin(x) / x);
}

/******************************************************************************
 * the first derivative of desired amplitude response functions for four      *
 * cases of filter design                                                     *
 ******************************************************************************/
static Float dD1(Float)
{
  return (0);
}

static Float dD2(Float x)
{
  Float a;
  a = sin(x);
  return (-Desire(x) * cos(x) / a / a);
}

static Float dD3(Float x)
{
  Float a;
  a = cos(x / 2);
  return (Desire(x) * sin(x / 2) / a / a / 2);
}

static Float dD4(Float x)
{
  Float a;
  a = sin(x / 2);
  return (-Desire(x) * cos(x / 2) / a / a / 2);
}

/******************************************************************************
 * the second derivative of desired amplitude response functions for four     *
 * cases of filter design                                                     *
 ******************************************************************************/
static Float d2D1(Float)
{
  return (0);
}

static Float d2D2(Float x)
{
  Float a = sin(x), b = cos(x);
  return (Desire(x) * (1 + b * b) / a / a / a);
}

static Float d2D3(Float x)
{
  Float a = sin(x / 2), b = cos(x / 2);
  return (Desire(x) * (1 + a * a) / b / b / b / 4);
}

static Float d2D4(Float x)
{
  Float a = sin(x / 2), b = cos(x / 2);
  return (Desire(x) * (1 + b * b) / a / a / a / 4);
}

/******************************************************************************
 * Initialize extremals (fill in Extrm), and fill in W                        *
 ******************************************************************************/
static void InitExtremals(void)
{
  Float W0;
  int i, L;
  for(i = 1, W0 = 0; i <= GetBandsNo(FIR); i++)
    W0 += FIR[i].GetHigh() - FIR[i].GetLow();
  W0 /= r + 1 - GetBandsNo(FIR);
  W.SetDims(1, GetBandsNo(FIR));
  Extrm.SetDims(1, GetBandsNo(FIR));
  for(i = 1, L = 0; i < GetBandsNo(FIR); i++)
  {
    int m = (int)((FIR[i].GetHigh() - FIR[i].GetLow()) / W0 + 0.5);
    W[i] = (FIR[i].GetHigh() - FIR[i].GetLow()) / m;
    Extrm[i].SetDims(1, m + 1);
    for(int j = 1; j <= m; j++)
      Extrm[i][j] = FIR[i].GetLow() + (j - 1) * W[i];
    Extrm[i][m + 1] = FIR[i].GetHigh();
    L += m + 1;
  }
  L = r - L;
  W[GetBandsNo(FIR)] = (FIR[GetBandsNo(FIR)].GetHigh() - FIR[GetBandsNo(FIR)].GetLow()) / L;
  Extrm[GetBandsNo(FIR)].SetDims(1, L + 1);
  for(int j = 1; j <= L; j++)
    Extrm[GetBandsNo(FIR)][j] = FIR[GetBandsNo(FIR)].GetLow() + (j - 1) * W[GetBandsNo(FIR)];
  Extrm[GetBandsNo(FIR)][L + 1] = FIR[GetBandsNo(FIR)].GetHigh();
}

/******************************************************************************
 * calculate ALFA and BETA coefficients and put them into Alfa and Beta       *
 ******************************************************************************/
static void CalcAlfaBeta(void)
{
  int i, j, k, m;
  Float x, b, y;
  Beta.SetDims(1, Hi(cosExtrm));
  Alfa.SetDims(1, Hi(cosExtrm));
  y = cosExtrm[Hi(cosExtrm)][Hi(cosExtrm[Hi(cosExtrm)])];
  for(m = 1; m <= Hi(cosExtrm); m++)
  {
    Beta[m].SetDims(1, Hi(cosExtrm[m]));
    Alfa[m].SetDims(1, Hi(cosExtrm[m]));
    for(k = 1; k <= Hi(cosExtrm[m]); k++)
    {
      x = cosExtrm[m][k];
      b = 1;
      for(j = 1; j <= Hi(cosExtrm); j++)
        for(i = 1; i <= ((j == Hi(cosExtrm)) ? Hi(cosExtrm[j]) - 1 : Hi(cosExtrm[j])); i++)
          if(i != k || j != m)
            b /= x - cosExtrm[j][i];
      Beta[m][k] = b;
      if(m != Hi(cosExtrm) || k != Hi(cosExtrm[Hi(cosExtrm)]))
        b /= x - y;
      Alfa[m][k] = b;
    }
  }
}

/******************************************************************************
 * calculate C coefficients and put them into C                               *
 ******************************************************************************/
static void CalcC(void)
{
  int k, m, q;
  C.SetDims(1, Hi(Extrm));
  for(m = 1, q = 0; m <= Hi(C); q += Hi(C[m++]))
  {
    C[m].SetDims(1, Hi(Extrm[m]));
    for(k = 1; k <= Hi(C[m]); k++)
      C[m][k] = Desire(Extrm[m][k]) - ((q + k - 1) % 2 ? -1 : 1) * Delta / Weight(Extrm[m][k]);
  }
}

/******************************************************************************
 * calculate error for current extremals                                      *
 ******************************************************************************/
static Float CalcDelta(void)
{
  int k, m, q;
  Float licz = 0, mian = 0;
  for(m = 1, q = 0; m <= Hi(Extrm); q += Hi(Extrm[m++]))
    for(k = 1; k <= Hi(Extrm[m]); k++)
    {
      licz += Alfa[m][k] * Desire(Extrm[m][k]);
      mian += (((q + k - 1) % 2) ? -1 : 1) * Alfa[m][k] / Weight(Extrm[m][k]);
    }
  return (licz / mian);
}

/******************************************************************************
 * calculate approximation for current extremals at given point omega         *
 ******************************************************************************/
static Float CalcPc(Float omega)
{
  int k, m;
  for(m = 1; m <= Hi(Extrm); m++)
    for(k = 1; k <= Hi(Extrm[m]); k++)
      if(Eq(omega, Extrm[m][k]))
        return (C[m][k]);
  double x = cos(omega), y, licz = 0, mian = 0;
  for(m = 1; m <= Hi(Extrm); m++)
    for(k = 1; k <= (m == Hi(Extrm) ? Hi(Extrm[m]) - 1 : Hi(Extrm[m])); k++)
    {
      licz += (y = Beta[m][k] / (x - cosExtrm[m][k])) * C[m][k];
      mian += y;
    }
  return (licz / mian);
}

/******************************************************************************
 * calculate approximation for current extremals at given point w             *
 ******************************************************************************/
static Float CalcE(Float w)
{
  if(In(Extrm, w))
    return (fabs(Delta));
  else
    return (fabs(Weight(w) * (Desire(w) - CalcPc(w))));
}

/******************************************************************************
 * First derivative of Pc                                                     *
 ******************************************************************************/
static Float CalcdPc1(Float omega)
{
  int i, j, k, m;
  Float n, n1, d, d1, a, b, c, x;
  if(Eq(omega, 0) || Eq(omega, M_PI))
    return (0);
  for(j = 1; j <= Hi(Extrm); j++)
    for(i = 1; i <= (j == Hi(Extrm) ? Hi(Extrm[j]) - 1 : Hi(Extrm[j])); i++)
      if(Eq(omega, Extrm[j][i]))
      {
        a = C[j][i];
        b = cosExtrm[j][i];
        c = 0;
        for(m = 1; m <= Hi(Extrm); m++)
          for(k = 1; k <= (m == Hi(Extrm) ? Hi(Extrm[m]) - 1 : Hi(Extrm[m])); k++)
            if(k != i || m != j)
              c += Beta[m][k] * (a - C[m][k]) / (b - cosExtrm[m][k]);
        return (c * sin(Extrm[j][i]) / Beta[j][i]);
      }
  x = cos(omega);
  n = n1 = d = d1 = 0;
  for(m = 1; m <= Hi(Extrm); m++)
    for(k = 1; k <= (m == Hi(Extrm) ? Hi(Extrm[m]) - 1 : Hi(Extrm[m])); k++)
    {
      a = Beta[m][k];
      b = x - cosExtrm[m][k];
      c = C[m][k] * a;
      d += a / b;
      n += c / b;
      b *= b;
      d1 += a / b;
      n1 += c / b;
    }
  n1 *= x = sin(omega);
  d1 *= x;
  return ((n1 - d1 * n / d) / d);
}

/******************************************************************************
 * First derivative of E                                                      *
 ******************************************************************************/
static Float CalcG1(Float w)
{
  if(FType == OddDifferentiator)
  {
    Float sinww = Q5(w), Pcw = CalcPc(w);
    return (sgn(1 - sinww * Pcw) * ((Eq(w, 0) ? 0 : (sinww - cos(w)) / w * Pcw) - sinww * CalcdPc1(w)));
  }
  else
    return (sgn(Desire(w) - CalcPc(w)) * ((*__dD)(w) - CalcdPc1(w)));
}

/******************************************************************************
 * second derivative of Pc                                                    *
 ******************************************************************************/
static Float CalcdPc2(Float omega)
{
  int k, m;
  Float d, d2, n, n2, a, b, c, x;
  assert(Eq(omega, 0) || Eq(omega, M_PI));
  if(Eq(omega, 0) && In(Extrm, 0))
  {
    a = C[1][1];
    b = cosExtrm[1][1];
    c = 0;
    for(m = 1; m <= Hi(Extrm); m++)
      for(k = 1; k <= (m == Hi(Extrm) ? Hi(Extrm[m]) - 1 : Hi(Extrm[m])); k++)
        if(k != 1 || m != 1)
          c += Beta[m][k] * (a - C[m][k]) / (b - cosExtrm[m][k]);
    return (c / Beta[1][1]);
  }
  x = cos(omega);
  n = n2 = d = d2 = 0;
  for(m = 1; m <= Hi(Extrm); m++)
    for(k = 1; k <= (m == Hi(Extrm) ? Hi(Extrm[m]) - 1 : Hi(Extrm[m])); k++)
    {
      a = Beta[m][k];
      b = x - cosExtrm[m][k];
      c = C[m][k] * a;
      d += a / b;
      n += c / b;
      b *= b;
      d2 += a / b;
      n2 += c / b;
    }
  n2 *= x;
  d2 *= x;
  return ((n2 - d2 * n / d) / d);
}

/******************************************************************************
 * second derivative of E                                                     *
 ******************************************************************************/
static Float CalcG2(Float w)
{
  if(FType == OddDifferentiator)
    return (CalcE(W[1] / S) - CalcE(0));
  else
    return (sgn(Desire(w) - CalcPc(w)) * ((*__d2D)(w) - CalcdPc2(w)));
}

/******************************************************************************
 * calculate convergence parameter                                            *
 ******************************************************************************/
static Float CalcQ(void)
{
  Float x, max, min;
  int i, j;
  max = min = CalcE(NewExtrm[1][1]);
  for(j = 1; j <= Hi(NewExtrm); j++)
    for(i = j == 1 ? 2 : 1; i <= Hi(NewExtrm[j]); i++)
      if((x = CalcE(NewExtrm[j][i])) > max)
        max = x;
      else
        if(x < min)
          min = x;
  return (1 - min / max);
}

/******************************************************************************
 * selective search starting from extremum ekstr in band pasmo, use gradient  *
 * information (g1 - first, g2 - second derivative in this extremum), if new  *
 * extremum is found, add it to list L and return 1, else return 0            *
 ******************************************************************************/
static int Selective(Float g1, Float g2, int ekstr, int pasmo, List &L)
{
  Float w, wij, up, dw, e, x;
  const Float step = W[pasmo] / S;
  wij = Extrm[pasmo][ekstr];
  dw = ((ekstr == 1) ? FIR[pasmo].GetLow() : Extrm[pasmo][ekstr - 1]) + step;
  up = ((ekstr == Hi(Extrm[pasmo])) ? FIR[pasmo].GetHigh() : Extrm[pasmo][ekstr + 1]) - step;
  if((!Eq(wij, 0) && g1 >= 0) || (Eq(wij, 0) && g2 > 0))
    for(w = wij; w <= up; w += step)
    {
      int test;
      test = (e = CalcE(w)) >= fabs(Delta) - 1E-6;
      if(test && (x = w - step) >= FIR[pasmo].GetLow())
        test = test && (e >= CalcE(x));
      if(test && (x = w + step) <= FIR[pasmo].GetHigh())
        test = test && (e >= CalcE(x));
      if(test && !In(L, w))
        if(!(Eq(w, 0) && (FType == OddAntysymetrical || FType == EvenAntysymetrical)) ||
           !(Eq(w, M_PI) && (FType == OddAntysymetrical || FType == EvenSymetrical)))
        {
          L.Add(w);
          return (1);
        }
    }
  if((!Eq(wij, M_PI) && g1 <= 0) || (Eq(wij, M_PI) && g2 > 0))
    for(w = wij; w >= dw; w -= step)
    {
      int test;
      test = (e = CalcE(w)) >= fabs(Delta) - 1E-6;
      if(test && (x = w - step) >= FIR[pasmo].GetLow())
        test = test && (e >= CalcE(x));
      if(test && (x = w + step) <= FIR[pasmo].GetHigh())
        test = test && (e >= CalcE(x));
      if(test && !In(L, w))
        if(!(Eq(w, 0) && (FType == OddAntysymetrical || FType == EvenAntysymetrical)) ||
           !(Eq(w, M_PI) && (FType == OddAntysymetrical || FType == EvenSymetrical)))
        {
          L.Add(w);
          return (1);
        }
    }
  return (0);
}

/******************************************************************************
 * cubic interpolation starting from extremum i in band j, use                *
 * gradient information (g - first derivative in this extremum), if new       *
 * extremum is found, add it to list L and return 1, else return 0            *
 ******************************************************************************/
static int Cubic(Float g, int i, int j, List &L)
{
  Float w1, w2, w3, d, c, b, x;
  w1 = Extrm[j][i];
  if(Eq(g, 0))
    return (0);
  w3 = (g > 0) ? w1 + Q / 2 * (Extrm[j][i + 1] - w1) : w1 - Q / 2 * (w1 - Extrm[j][i - 1]);
  w2 = (w1 + w3) / 2;
  c = w1 - w2;
  x = ((CalcE(w2) - (b = fabs(Delta))) / c + g) / c;
  c = w1 - w3;
  d = (x - ((CalcE(w3) - b) / c + g) / c) / (w2 - w3);
  c = x - (2 * w1 + w2) * d;
  b = g - (2 * c + 3 * d * w1) * w1;
  if(3 * b * d >= c * c)
    return (0);
  x = c - sqrt(c * c - 3 * b * d);
  if(x < c)
    x = 2 * c - x;
  x /= -3 * d;
  if(w1 < w3)
  {
    if(x < w1 || x > w3)
      return (0);
  }
  else
    if(x > w1 || x < w3)
      return (0);
  if(CalcE(x) < fabs(Delta) - 1E-6)
    return (0);
  x = W[j] / S * (int)(x / (W[j] / S) + 0.5);
  if(In(L, x))
    return (0);
  if(!(Eq(x, 0) && (FType == OddAntysymetrical || FType == EvenAntysymetrical)) ||
     !(Eq(x, M_PI) && (FType == OddAntysymetrical || FType == EvenSymetrical)))
  {
    L.Add(x);
    return (1);
  }
  return (0);
}

/******************************************************************************
 * exhaustive search in band j from Curr point in list L until new extremum   *
 * found, or end of the band reached; if new extremum is found, add it to the *
 * list L and return 1, otherwise return 0                                    *
 ******************************************************************************/
static int Exhaustive(int j, List &L)
{
  Float e, w, w1, w2, x;
  const Float step = W[j] / S;
  w1 = L;
  w2 = +L - step;
  for(w = w1 + step; w <= w2; w += step)
  {
    int test;
    test = (e = CalcE(w)) >= fabs(Delta) - 1E-6;
    if(test && (x = w - step) >= FIR[j].GetLow())
      test = e >= CalcE(x);
    if(test && (x = w + step) <= FIR[j].GetHigh())
      test = e >= CalcE(x);
    if(test && !In(L, w))
    {
      L.InsertAfter(w);
      return (1);
    }
  }
  return (0);
}

/******************************************************************************
 * find new potential extremals using selective search or selective search    *
 * with cubic interpolation method, based on the value of ifc parameter       *
 ******************************************************************************/
static int ExtremalsSC(int ifc)
{
  List L;
  int i, j, e, ro;
  Float x, g1, g2;
  NewExtrm.SetDims(1, Hi(Extrm));
  for(j = 1, ro = 0; j <= Hi(Extrm); j++)
  {
    e = 0;
    !L;
    g1 = CalcG1(Extrm[j][1]);
    if(Eq(Extrm[j][1], FIR[j].GetLow()))
      if(j == 1 && Eq(FIR[1].GetLow(), 0))
        if((g2 = CalcG2(Extrm[j][1])) < 0)
        {
          e++;
          L.InsertAfter(Extrm[j][1]);
        }
        else
          e += Selective(g1, g2, 1, j, L);
      else
        if(g1 > 0)
          e += Selective(g1, 0, 1, j, L);
        else
        {
          e++;
          L.InsertAfter(Extrm[j][1]);
        }
    else
      e += Selective(g1, 0, 1, j, L);
    for(i = 2; i < Hi(Extrm[j]); i++)
    {
      g1 = CalcG1(Extrm[j][i]);
      if(ifc && Q >= 0.65 && Cubic(g1, i, j, L))
        e++;
      else
        e += Selective(g1, 0, i, j, L);
    }
    i = Hi(Extrm[j]);
    g1 = CalcG1(Extrm[j][i]);
    if(Eq(Extrm[j][i], FIR[j].GetHigh()))
      if(j == Hi(Extrm) && Eq(FIR[GetBandsNo(FIR)].GetHigh(), M_PI))
        if((g2 = CalcG2(Extrm[j][i])) < 0)
        {
          e++;
          L.InsertAfter(Extrm[j][i]);
        }
        else
          e += Selective(g1, g2, i, j, L);
      else
        if(g1 < 0)
          e += Selective(g1, 0, i, j, L);
        else
        {
          e++;
          L.InsertAfter(Extrm[j][i]);
        }
    else
      e += Selective(g1, 0, i, j, L);
    L.SetFirst();
    if(!(j == 1 && (FType == OddAntysymetrical || FType == EvenAntysymetrical)) &&
       !Eq(Extrm[j][1], FIR[j].GetLow()) && !Eq(L, FIR[j].GetLow()) &&
       (x = CalcE(FIR[j].GetLow())) > CalcE(FIR[j].GetLow() + W[j] / S) &&
       x >= fabs(Delta) - 1E-6)
    {
      e++;
      L.InsertBefore(FIR[j].GetLow());
    }
    L.SetLast();
    if(!(j == GetBandsNo(FIR) && (FType == OddAntysymetrical || FType == EvenSymetrical)) &&
       !Eq(Extrm[j][Hi(Extrm[j])], FIR[j].GetHigh()) &&
       !Eq(L, FIR[j].GetHigh()) &&
       (x = CalcE(FIR[j].GetHigh())) > CalcE(FIR[j].GetHigh() - W[j] / S) &&
       x >= fabs(Delta) - 1E-6)
    {
      e++;
      L.InsertAfter(FIR[j].GetHigh());
    }
    L.InsertAfter(FIR[j].GetHigh());
    L.SetFirst();
    L.InsertBefore(FIR[j].GetLow());
    while(++L)
      if(L - -L > R * W[j])
      {
        --L;
        if(Exhaustive(j, L))
          e++;
        else
          ++L;
      }
    L.SetFirst();
    NewExtrm[j].SetDims(1, e);
    for(i = 1; i <= e; i++)
    {
      ++L;
      NewExtrm[j][i] = L;
    }
    ro += e;
  }
  return (ro);
}

/******************************************************************************
 * find new potential extremals using exhaustive search method                *
 ******************************************************************************/
static int ExtremalsE(void)
{
  List L;
  int i, j, e, ro;
  NewExtrm.SetDims(1, Hi(Extrm));
  for(j = 1, ro = 0; j <= Hi(Extrm); j++)
  {
    !L;
    e = 2;
    L.Add(FIR[j].GetLow());
    L.Add(FIR[j].GetHigh());
    L.SetFirst();
    while(++L)
    {
      --L;
      if(Exhaustive(j, L))
        e++;
      else
        ++L;
    }
    L.SetLast();
    if(Eq(L, M_PI) && (FType == OddAntysymetrical || FType == EvenSymetrical))
      e--;
    L.SetFirst();
    if(Eq(L, 0) && (FType == OddAntysymetrical || FType == EvenAntysymetrical))
    {
      --e;
      ++L;
    }
    NewExtrm[j].SetDims(1, e);
    for(i = 1; i <= e; ++i, ++L)
      NewExtrm[j][i] = L;
    ro += e;
  }
  return (ro);
}

/******************************************************************************
 * kind of shell for previous two functions                                   *
 ******************************************************************************/
static int Extremals(SearchTypes stype)
{
  switch(stype)
  {
    case ExhaustiveOnly:
      return (ExtremalsE());
    case UseSelective:
      return (ExtremalsSC(0));
    case UseSelectCubic:
      return (ExtremalsSC(1));
    default:
      fail(Wrong search type in Extremals);
      return (-1);
  }
}

/******************************************************************************
 * reject superfluous potential extremals                                     *
 ******************************************************************************/
static void Reject(int ro, int r)
{
  if(GetBandsNo(FIR) > 1)
  {
    struct Nfo
    {
      Float Error;
      int Band;
    } *Tbl;
    int i, j;
    Tbl = new Nfo[Hi(NewExtrm)];
    for(j = 1; j <= Hi(NewExtrm); j++)
    {
      Float e = 0;
      Tbl[j - 1].Band = j;
      for(i = 1; i <= Hi(NewExtrm[j]); i++)
        e += CalcE(NewExtrm[j][i]);
      Tbl[j - 1].Error = e / Hi(NewExtrm[j]);
    }
    for(j = 1; j < Hi(Extrm); j++)
      for(i = 1; i < Hi(Extrm); i++)
        if(Tbl[i - 1].Error > Tbl[i].Error)
        {
          Nfo temp = Tbl[i - 1];
          Tbl[i - 1] = Tbl[i];
          Tbl[i] = temp;
        }
    for(j = 0; ro > r + 1; j = (j + 1) % (Hi(NewExtrm) - 1))
    {
      Float min, min1;
      int ind, min_i, max_i, curr_band;
      curr_band = Tbl[j].Band;
      min_i = Eq(NewExtrm[curr_band][1], FIR[curr_band].GetLow()) ? 2 : 1;
      max_i = Hi(NewExtrm[curr_band]);
      if(Eq(NewExtrm[curr_band][Hi(NewExtrm[curr_band])], FIR[curr_band].GetHigh()))
        max_i--;
      min = CalcE(NewExtrm[curr_band][min_i]);
      ind = min_i;
      for(i = min_i + 1; i <= max_i; i++)
        if((min1 = CalcE(NewExtrm[curr_band][i])) < min)
        {
          ind = i;
          min = min1;
        }
      Vector V(1, Hi(NewExtrm[curr_band]) - 1);
      for(i = 1; i < ind; i++)
        V[i] = NewExtrm[curr_band][i];
      for(i = ind + 1; i <= Hi(NewExtrm[curr_band]); i++)
        V[i - 1] = NewExtrm[curr_band][i];
      NewExtrm[curr_band] = V;
      ro--;
    }
    delete Tbl;
  }
  else
    while(ro > r + 1)
    {
      Float min, min1;
      int ind, min_i, max_i;
      min_i = Eq(NewExtrm[1][1], FIR[1].GetLow()) ? 2 : 1;
      max_i = Hi(NewExtrm[1]);
      if(Eq(NewExtrm[1][Hi(NewExtrm[1])], FIR[1].GetHigh()))
        max_i--;
      min = CalcE(NewExtrm[1][min_i]);
      ind = min_i;
      int i;
      for(i = min_i + 1; i <= max_i; i++)
        if((min1 = CalcE(NewExtrm[1][i])) < min)
        {
          ind = i;
          min = min1;
        }
      Vector V(1, Hi(NewExtrm[1]) - 1);
      for(i = 1; i < ind; i++)
        V[i] = NewExtrm[1][i];
      for(i = ind + 1; i <= Hi(NewExtrm[1]); i++)
        V[i - 1] = NewExtrm[1][i];
      NewExtrm[1] = V;
      ro--;
    }
}

/******************************************************************************
 * setup Remez optimization for filter AFIR of type AT and with required      *
 * length in Alen                                                             *
 ******************************************************************************/
void InitRemez(const Filter &AFIR, int Alen, FilterTypes AT)
{
  if(Alen % 2)
  {
    if(AT == EvenSymetrical || AT == EvenAntysymetrical)
      Alen++;
  }
  else
    if(AT == OddSymetrical || AT == OddAntysymetrical || AT == OddDifferentiator)
      Alen++;
  FLength = Alen;
  r = Alen / 2;
  FIR = AFIR;
  switch(FType = AT)
  {
    case OddSymetrical:
      r++;
      __Q = Q1;
      __dD = dD1;
      __d2D = d2D1;
      break;
    case OddAntysymetrical:
      __Q = Q2;
      __dD = dD2;
      __d2D = d2D2;
      if(Eq(FIR[GetBandsNo(FIR)].GetHigh(), M_PI))
        FIR[GetBandsNo(FIR)].SetHigh(M_PI - Skipper * (M_PI - FIR[GetBandsNo(FIR)].GetLow()));
      if(Eq(FIR[1].GetLow(), 0))
        FIR[1].SetLow(Skipper * FIR[1].GetHigh());
      break;
    case EvenSymetrical:
      __Q = Q3;
      __dD = dD3;
      __d2D = d2D3;
      if(Eq(FIR[GetBandsNo(FIR)].GetHigh(), M_PI))
        FIR[GetBandsNo(FIR)].SetHigh(M_PI - Skipper * (M_PI - FIR[GetBandsNo(FIR)].GetLow()));
      break;
    case EvenAntysymetrical:
      __Q = Q4;
      __dD = dD4;
      __d2D = d2D4;
      if(Eq(FIR[1].GetLow(), 0))
        FIR[1].SetLow(Skipper * FIR[1].GetHigh());
      break;
    case OddDifferentiator:
      __Q = Q5;
      FIR[1].SetTrans(1);
      FIR[1].SetErrorRatio(1);
      break;
  }
  Q = 1;
  InitExtremals();
  cosExtrm = cos(Extrm);
  CalcAlfaBeta();
  Delta = CalcDelta();
  CalcC();
}

/******************************************************************************
 * single Remez iteration; return 0 if Q satisfies our requirments, -1 if we  *
 * should continue (Q doesn't satisfy our requirments), -2 when routine       *
 * cannot find enough new extremals and we must terminate                     *
 ******************************************************************************/
int RemezIteration(SearchTypes s)
{
  int ro;
  ro = Extremals(s);
  if(ro <= r)
    return(-2);
  Reject(ro, r);
  Q = CalcQ();
  Extrm = NewExtrm;
  cosExtrm = cos(Extrm);
  CalcAlfaBeta();
  Delta = CalcDelta();
  CalcC();
  if(Q < RequiredQ)
    return (0);
  else
    return (-1);
}

/******************************************************************************
 * perform Remez optimization using s search type and stop if maxiter number  *
 * of iterations is exceeded; return 0 if satisfactory solution is found, -1  *
 * when maxiter number of iterations is exceeded and -2 when cannot find      *
 * enough new extremals                                                       *
 ******************************************************************************/
int Remez(SearchTypes s, int maxiter)
{
  int i;
  for(i = 1; i <= maxiter; i++)
    switch(RemezIteration(s))
    {
      case  0: return (0);
      case -2: return (-2);
    };
  return (-1);
}

/******************************************************************************
 * calculate filter impulse response (filter coefficients) based on values    *
 * from the last optimization                                                 *
 ******************************************************************************/
Vector CalcH(void)
{
  Vector h(0, FLength - 1), a(0, r - 1), p(0, r - 1);
  const Float omega = 2 * M_PI / FLength;
  Float x;
  int i, j;
  for(i = 0; i < r; i++)
    p[i] = CalcPc(omega * i);
  for(i = 0; i < r; i++)
  {
    for(j = 1, x = 0; j < r; j++)
      x += p[j] * cos(omega * i * j);
    a[i] = (p[0] + 2 * x) / FLength;
  }
  switch(FType)
  {
    case OddSymetrical:
      for(i = 0; i < r; i++)
        h[i] = h[FLength - i - 1] = a[r - 1 -i];
      break;
    case OddDifferentiator:
    case OddAntysymetrical:
      h[FLength - 1] = -(h[0] = a[r - 1] / 2);
      h[FLength - 2] = -(h[1] = a[r - 2] / 2);
      for(i = 2; i < r; i++)
        h[FLength - 1 - i] = -(h[i] = (a[r - i - 1] - a[r - i + 1]) / 2);
      h[r] = 0;
      break;
    case EvenSymetrical:
      h[FLength - 1] = h[0] = a[r - 1] / 2;
      for(i = 1; i < r; i++)
        h[FLength - 1 - i] = h[i] = (a[r - i - 1] + a[r - i]) / 2;
      break;
    case EvenAntysymetrical:
      h[FLength - 1] = -(h[0] = a[r - 1] / 2);
      for(i = 1; i < r; i++)
        h[FLength - 1 - i] = -(h[i] = (a[r - i - 1] - a[r - i]) / 2);
      break;
  }
  return (h);
}
